/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Timer;

import file.Configurable;
import file.Configuration;

/**
 *
 * @author 708 Carl Burger
 */

/**
 * Class implements a robot drivetrain controller.
 */

public class DriveController implements Configurable {

    private static DriveController m_instance = null;

    // the robot is always moving in one of the following modes

    private static int      m_mode;

    public static final int kFindLine    = 1;
    public static final int kFollowLine  = 2;
    public static final int kManual      = 3;
    public static final int kRotate      = 4;
    public static final int kStraightFwd = 5;
    public static final int kStraightRev = 6;

    public static final int kLeft        = 0;
    public static final int kRight       = 1;
    public static final int kStraight    = 2;

    double maximumDriveSpeed   = 0;
    double maximumRotationRate = 0;

    private PIDController708 rotationControl;
    private double tolerance     = 0.5;   // 0.5% of (max - min) = 1.8 degrees
    private double minimumInput  = -180;  // gyro minimum normalized angle
    private double maximumInput  = 180;   // gyro maximum normalized angle
    private double minimumOutput = -1.0;  // motor minimum speed
    private double maximumOutput = 1.0;   // motor maximum speed
    private double proportional  = 0.005; // needs to be tuned
    private double integral      = 0.001; // needs to be tuned
    private double differential  = 0.0;   // needs to be tuned

    Timer  timer;

    double lineFindTimeout   = 3.0;
    double lineFollowTimeout = 4.0;
    double rotationTimeout   = 4.0;
    double backupTimeout     = 0.3;

    double backupSpeed       = 0.5;
    double lineFindSpeed     = 0.6;
    double lineFollowSpeed   = 0.5;
    double lineFollowGain    = 0.65;

    int rotationCoefficient = 1;
    boolean invertAxes      = false;

    // PIDsource could be WallDetector (1 or 2 ultrasonics for proximity),
    // encoders (distance), or a gyroscope.  PIDOutputProxy implements PIDOutput
    // and has a getPidOutput() method to allow more than one PIDcontroller to
    // control the drivetrain
    PIDOutputProxy zAxis;  // holds PID controller output with gyro as the source

    Drivetrain     drivetrain;
    Gyroscope      gyroscope;
    LineFollower   lineFollower;


    /**
     *  Get an instance of the DriveController
     */
    public static DriveController getInstance() {
        if (m_instance == null) {
            m_instance = new DriveController();
            Configuration.getInstance().register("DriveController", m_instance);
        }
        return m_instance;
    }

    /**
     * Allocate a DriveController object
     */

    private DriveController () {

        m_mode = kManual;  // not moving

        lineFollower = LineFollower.getInstance();

        gyroscope = Gyroscope.getInstance();

        // time the operation
        timer = new Timer();

        zAxis = new PIDOutputProxy();

        drivetrain = Drivetrain.getInstance();

        rotationControl = new PIDController708(proportional, integral, differential,
                                            gyroscope, zAxis);
        rotationControl.setInputRange(minimumInput, maximumInput);
        rotationControl.setOutputRange(minimumOutput, maximumOutput);
        rotationControl.setTolerance(tolerance);
    }

    /**
     * This function is called each time a new mode is entered (except manual).
     */
    public void timerReset() {

        timer.start();
        timer.reset();
    }

    /**
     * This function is called to back up the robot for a configured time.
     */
    public void backup() {

        m_mode = kStraightRev;

        timerReset();

        System.out.println("Back up " + backupSpeed);
        drivetrain.operatorDrive(0, backupSpeed, 0, 0, 0, true);
    }


    /**
     * Follow a line using the path parameter.
     * @param path is straight | left | right
     */
    public synchronized void followLine (String path) {

        timer.stop();

        if (m_mode == kFollowLine) {

            lineFollower.disable();
            m_mode = kManual;
        }
        else {
            if (m_mode == kFindLine) {

                drivetrain.operatorDrive(0, 0, 0, 0, 0, true);  // stop
                System.out.println("followLine drivetrain stopped");

            } else if (m_mode == kRotate) {

                rotationControl.reset();  // disable the rotation controller
            }

            if (lineFollower.getSensorStatus() != 0) {
                m_mode = kFollowLine;
                timerReset();             // in case end of line not found

                //speed=0.5 gain=0.65 proximity=20  from autoscript (needs to be configurable)
                lineFollower.setControls(path, lineFollowSpeed, lineFollowGain, 20);
                lineFollower.enable();     // start following the line
            }
            else {
                m_mode = kManual;
                System.out.println("followLine sensors were 0");
            }
        }
    }

    /**
     * Find a line using the direction parameter.
     * @param direction is left | right
     */
    public synchronized void findLine (int direction) {

        if (m_mode == kFindLine) {

            timer.stop();
            drivetrain.operatorDrive(0, 0, 0, 0, 0, true);  // stop
            m_mode = kManual;
        }
        else {
            if (m_mode == kFollowLine) {

                lineFollower.disable();

            } else if (m_mode == kRotate) {

                rotationControl.reset();  // disable the rotation controller
            }

            m_mode = kFindLine;

            timerReset();  // in case line not found

            if (lineFollower.getSensorStatus() == 0) {

                double xAxis = (direction == kLeft) ? -lineFindSpeed : lineFindSpeed;
                System.out.println("Finding line " + direction);
                drivetrain.operatorDrive(xAxis, 0, 0, 0, 0, true);
            }
            // if already on the line, leave m_mode as kFindLine; checkProgress will call followLine
        }
    }


    /**
     * Rotate robot to the commanded heading.
     * @param heading is -180 to 180 degrees
     */
    public synchronized void setHeading (int heading) {

        if (m_mode == kRotate) {

            timer.stop();
            rotationControl.reset();
            m_mode = kManual;
        }
        else {
            if (m_mode == kFollowLine) {

                lineFollower.disable();

            } else if (m_mode == kFindLine) {

                drivetrain.operatorDrive(0, 0, 0, 0, 0, true);  // stop
            }

            m_mode = kRotate;

            // set timer in case desired heading not reached
            timerReset();

            rotationControl.setSetpoint(heading);
//            System.out.println("setHeading" + rotationControl.onTarget() + rotationControl.isEnable());
            if (rotationControl.isEnable() == false) {
                System.out.println("setHeading PidController enabled");
                rotationControl.reset();
                rotationControl.enable(); // start calculating PIDOutput values

            }
        }
    }


    /**
     * Set the mode parameter.
     * @param x is -1 to 1
     * @param y is -1 to 1
     * @param z is -1 to 1
     * @param mag is -1 to 1
     * @param direction is ???
     */
    public synchronized void drive (double x, double y, double z, double mag, double dir, boolean relSteering) {

        z = z * rotationCoefficient;
        x = invertAxes ? -x : x;
        y = invertAxes ? -y : y;

        if (m_mode == kManual) {

            drivetrain.operatorDrive(x, y, z, mag, dir, relSteering);

        }
        else {

            // check if the operator is manually overriding an in-progress automated operation

            if (Math.abs(x) > 0.1 || Math.abs(y) > 0.1 || Math.abs(z) > 0.1 ) {

                if (m_mode == kFollowLine) {

                    lineFollower.disable();

                } else if (m_mode == kFindLine || m_mode == kStraightRev) {

                    // nothing to disable

                } else if (m_mode == kRotate) {

                    rotationControl.reset();  // disable the rotate thread

                }

                timer.stop();
                System.out.println("Operator override of auto operation " + m_mode);
                m_mode = kManual;
                drivetrain.operatorDrive(x, y, z, mag, dir, relSteering);

            }
            else {
                // driver joystick isn't registering a meaningful movement, so
                // allow the automated operation to continue
            }
        }
    }

    /**
     * Check the progress of the current drive operation.
     */

    public synchronized void checkProgress () {

        if (m_mode == kFollowLine) {

            if (lineFollower.onTarget() || (timer.get() > lineFollowTimeout)) {

                timer.stop();
                lineFollower.disable();
                backup();
//                m_mode = kManual;
            }

        } else if (m_mode == kFindLine) {

            System.out.println("DriveController CheckProgress FindLine " + lineFollower.getSensorStatus());

            if (lineFollower.getSensorStatus() != 0) {
                if (lineFollower.getSensorStatus() >=4) {
                    followLine("right");  // left sensor sees line
                } else {
                    followLine("left");
                }

            } else if (timer.get() > lineFindTimeout) {
                // disable the line finder thread
                System.out.println("lineFindTimeout");
                timer.stop();
                drivetrain.operatorDrive(0, 0, 0, 0, 0, true);  // stop
                m_mode = kManual;
            }

        } else if (m_mode == kRotate) {

            if (rotationControl.onTarget() || (timer.get() > rotationTimeout)) {
                rotationControl.reset();
                drivetrain.operatorDrive(0, 0, 0, 0, 0, true);  // stop
                System.out.println("rotationControl stopped");
                m_mode = kManual;
            }
            else {
                double rotation = zAxis.getPidOutput() * rotationCoefficient;
                drivetrain.operatorDrive(0, 0, rotation, 0, 0, true);
            }

        } else if (m_mode == kManual) {

            // nothing to do

        } else if (m_mode == kStraightRev) {

//            System.out.println("Backing up " + timer.get());
            // keep feeding the drivetrain in case the LineFollower detects the T
            // but is interrupted before it stops the robot.  Otherwise, backup()
            // starts the robot backing up, and then LineFollower stops it
            drivetrain.operatorDrive(0, backupSpeed, 0, 0, 0, true);

            if (timer.get() > backupTimeout) {
                
                // rotate to be perpendicular with grid
                System.out.println("Start rotating to align");
                timer.stop();
                drivetrain.operatorDrive(0, 0, 0, 0, 0, true);  // stop
                setHeading(0);
            }
        }
    }

    public void outputStatus(){
        drivetrain.outputStatus();
        lineFollower.outputStatus();
    }

    public void recordStatus(){
        drivetrain.recordStatus();
        lineFollower.recordStatus();
    }


//    public void monitorRotation() {
//
//        // if the robot is moving, check if it reached the commanded position
//
////        System.out.println("monitorRotation " + rotationControl.isEnable());
//        if (rotationControl.isEnable()) {
//
//            System.out.println("Drivetrain monitorRotation error " + rotationControl.getError());
//            if (rotationControl.onTarget()) {
//                // stop running the PIDController. Sets output to 0 before stopping
//                rotationControl.disable();
//
//                System.out.println("Drivetrain monitorRotation stopped");
//            }
//        }
//    }


    /**
     * Return true if the robot has completed the operation or timed-out
     * @return true if the robot is in position
     */
    public boolean onTarget() {
        return (m_mode == kManual);
    }


    public void update(String name, String[] values) {
        if (name.equals("Maximum.Drive.Speed")) {
            maximumDriveSpeed = Double.parseDouble(values[0]);

        } else if (name.equals("Maximum.Rotation.Rate")) {
            maximumRotationRate = Double.parseDouble(values[0]);

        } else if (name.equals("Line.Find.Timeout")) {
            lineFindTimeout = Double.parseDouble(values[0]);

        } else if (name.equals("Line.Find.Speed")) {
            lineFindSpeed = Double.parseDouble(values[0]);

        } else if (name.equals("Line.Follow.Timeout")) {
            lineFollowTimeout = Double.parseDouble(values[0]);
            
        } else if (name.equals("Line.Follow.Speed")) {
            lineFollowSpeed = Double.parseDouble(values[0]);
            
        } else if (name.equals("Line.Follow.Gain")) {
            lineFollowGain = Double.parseDouble(values[0]);

        } else if (name.equals("Backup.Timeout")) {
            backupTimeout = Double.parseDouble(values[0]);

        } else if (name.equals("Backup.Speed")) {
            backupSpeed = Double.parseDouble(values[0]);

        } else if (name.equals("Invert.Axes")) {
            invertAxes = values[0].equals("true");

        } else if (name.equals("Rotation.Coefficient")) {
            rotationCoefficient = Integer.parseInt(values[0]);

        } else if (name.equals("Rotation.Timeout")) {
            rotationTimeout = Double.parseDouble(values[0]);

        } else if (name.equals("Rotation.Tolerance")) {
            tolerance = Double.parseDouble(values[0]);
            rotationControl.setTolerance(tolerance);

        } else if (name.equals("Rotation.Mimimum.Output")) {
            minimumOutput = Double.parseDouble(values[0]);
            rotationControl.setOutputRange(minimumOutput, maximumOutput);

        } else if (name.equals("Rotation.Maximum.Output")) {
            maximumOutput = Double.parseDouble(values[0]);
            rotationControl.setOutputRange(minimumOutput, maximumOutput);

        } else if (name.equals("Rotation.Proportional")) {
            proportional = Double.parseDouble(values[0]);
            rotationControl.setPID(proportional, integral, differential);

        } else if (name.equals("Rotation.Integral")) {
            integral = Double.parseDouble(values[0]);
            rotationControl.setPID(proportional, integral, differential);

        } else if (name.equals("Rotation.Differential")) {
            differential = Double.parseDouble(values[0]);
            rotationControl.setPID(proportional, integral, differential);
        }
    }
}